#include <IMU.hpp>
#include <iostream>
#include <unistd.h>
#include <chrono>
#include <thread>

using namespace std;

int main(int argc, char **argv)
{
    cout << "IMU test" << endl;
    IMU imu;
    if (!imu.InitDevice("/dev/ttyUSB1", 115200))
    {
        cout << "IMU init fail" << endl;
        return -1;
    }
    while (true)
    {
        IMUState state = imu.GetState();
        cout.clear();
        cout << state.PrintState() << endl;
        auto end = chrono::steady_clock::now();
        this_thread::sleep_for(chrono::milliseconds(500));
    }
    return 0;
}
